//
// Created by rebeater on 4/27/21.
//

#include <stdio.h>
#include <memory.h>
#include "agilex.h"
#include "rtcommon.h"
#define MAX_VELOCITY 3.0
#define MAC_ANGULAR 2.5235

/*CRC编码输出*/
int agilex_encode(uint8_t buffer[], NavOutput *nav) {
    int sz = sizeof(NavOutput);
    buffer[0] = 0xAA;
    buffer[1] = 0x55;/*帧*/
    memcpy(buffer + 2, nav, sz);
    uint32_t crc = crc_checksum(&buffer[0], sz + 2);
//    printf("%X\n", crc);
    buffer[sz + 2] = (crc >> 24) & 0xFF;
    buffer[sz + 3] = (crc >> 16) & 0xFF;
    buffer[sz + 4] = (crc >> 8) & 0xFF;
    buffer[sz + 5] = (crc) & 0xFF;
    return sz + 6;
}

/**
 * CAN数据校验
 * @param id
 * @param rxdata
 * @param len
 * @return
 */
uint8_t agilex_check_sum(uint16_t id, const uint8_t rxdata[8], uint8_t len) {
    uint8_t checksum = 0x00;
    checksum = (uint8_t) (id & 0x00ffu) + (uint8_t) (id >> 8u) + len;
    for (int i = 0; i < (len - 1); i++) {
        checksum += rxdata[i];
    }
    return checksum;
}



/**
 * can data check
 * @param id
 * @param rxdata
 * @param len
 * @return
 */
uint8_t agilex_check(uint16_t id, const uint8_t rxdata[8], uint8_t len) {
    uint8_t checksum = agilex_check_sum(id,rxdata,len);
    return checksum==rxdata[len-1];
}
int agilexGetRobotInfo(RobotInfo *robotInfo ,const FDCAN_RxHeaderTypeDef *header,const uint8_t data[8]){
    if(!agilex_check(header->Identifier,data,header->DataLength)){
        return -1;/**/
    }
    robotInfo->state = data[0];
    robotInfo->mode = data[1];
    robotInfo->battery = (((uint16_t) data[2] << 8u) + data[3]) / 10.0f;
    robotInfo->error = data[4];
    return 0;
}
int agilexGetVelocity(Velocity *velocity,const FDCAN_RxHeaderTypeDef *header,const uint8_t data[8]){
    if(!agilex_check(header->Identifier,data,8)){
        return -1;/**/
    }
    velocity->forward = ((int16_t) (data[0] << 8 | data[1])) / 1000.0f;
    velocity->angular = ((int16_t) (data[2] << 8 | data[3])) / 1000.0f;
    return 0;
}
FDCAN_TxHeaderTypeDef TxHeader;
uint8_t cmd_cnt = 0;
void FDCAN_Config() {
    FDCAN_FilterTypeDef sFilterConfig;
    sFilterConfig.IdType = FDCAN_STANDARD_ID;
    sFilterConfig.FilterIndex = 0;
    sFilterConfig.FilterType = FDCAN_FILTER_MASK;
    sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
    /*mask 只需要接受0x131速度信息和0x151机器人信息，所以使用0b1111 1001 1111作为mask，0x79F*/
    sFilterConfig.FilterID1 = AGILEX_VELOCITY_IDENTIFIER;
    sFilterConfig.FilterID2 = 0x79F; /* For acceptance, MessageID and FilterID1 must match exactly */
    HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig);
    /* Configure global filter to reject all non-matching frames */
    HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE);

    /* Configure Rx FIFO 0 watermark to 2 */
    HAL_FDCAN_ConfigFifoWatermark(&hfdcan1, FDCAN_CFG_RX_FIFO0, 2);

    /* Activate Rx FIFO 0 watermark notification */
    HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);


    TxHeader.Identifier = 0x130;
    TxHeader.IdType = FDCAN_STANDARD_ID;
    TxHeader.TxFrameType = FDCAN_DATA_FRAME;
    TxHeader.DataLength = FDCAN_DLC_BYTES_8;
    TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    TxHeader.BitRateSwitch = FDCAN_BRS_OFF;
    TxHeader.FDFormat = FDCAN_CLASSIC_CAN;
    TxHeader.TxEventFifoControl = FDCAN_STORE_TX_EVENTS;
    TxHeader.MessageMarker = 0x52;
    cmd_cnt = 0;
}

int sendVelocity(const Velocity *vel){
    /* Configure Tx buffer message */
    uint8_t TxData[8]={0x01,0x00,0x0a,0x00,0x00,0x00,0x00,0x44};
    TxData[0] = 0x01;
    TxData[1] = 0x00;
    TxData[2] = (int)(100.0*vel->forward/MAX_VELOCITY);
    TxData[3] =(int)(100.0*vel->angular/MAC_ANGULAR);
    TxData[4] = 0x00;
    TxData[5] = 0x00;
    TxData[6] = cmd_cnt;
    cmd_cnt ++;
    TxData[7] =  agilex_check_sum(TxHeader.Identifier,TxData,8);
   if( HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData)!= HAL_OK){
       printf("transmit failed\n");

   };
    return 0;
}